feat(obstacle_cruise_planner): support pointcloud-based obstacles #6907

mitukou1109 · 2024-05-01T00:48:15Z

Description

Make obstacle_cruise_planner compatible with sensor_msgs::msg::PointCloud2-based obstacles.

The node subscribes to a sensor_msgs::msg::PointCloud2 topic and converts the point cloud into obstacles by the following steps:

transform the cloud into the trajectory frame
downsample the cloud by voxel grid and apply Euclidean clustering to get candidates of obstacles
check if each point in a cluster is within the stop / slow down margin
when any points are inside the stop / slow down area, create a new obstacle object containing:
- the nearest point from the ego for stop obstacles
- the nearest and the farthest from the ego for slow down obstacles

Stop and slow down planning are performed with the same trajectory generation algorithm as for ML-based (autoware_perception_msgs::msg::PredictedObject) obstacles.

Cruising and yielding are not supported since point cloud-based obstacles have no velocity information.

Margin adjustment on curves (#4952) is also unsupported due to the following reason:

Point cloud-based obstacles do not have shape information.
Unlike PredictedObject polygons, point clouds do not necessarily capture the entire obstacle. This could lead the margin to be smaller than in the ML-based case.

launch PR: autowarefoundation/autoware_launch#980

Tests performed

Psim

ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

with the following changes to obstacle_cruise_planner.param.yaml:

/**:
  ros__parameters:
    common:
     stop_obstacle_type:
        unknown: false
        car: false
        truck: false
        bus: false
        trailer: false
        motorcycle: false
        bicycle: false
        pedestrian: false
        pointcloud: true

     slow_down_obstacle_type:
        unknown: false
        car: false
        truck: false
        bus: false
        trailer: false
        motorcycle: false
        bicycle: false
        pedestrian: false
        pointcloud: true

    behavior_determination:
      pointcloud_search_radius: 5.0
      pointcloud_voxel_grid_x: 0.05
      pointcloud_voxel_grid_y: 0.05
      pointcloud_voxel_grid_z: 100000.0
      pointcloud_cluster_tolerance: 1.0
      pointcloud_min_cluster_size: 1
      pointcloud_max_cluster_size: 100000

Screencast.from.2024.06.27.16.40.32.webm

Evaluator:

w/ point cloud handling disabled: https://evaluation.tier4.jp/evaluation/reports/a6893361-4d9d-567f-8a2b-e92f49148286?project_id=prd_jt
w/ both ML-based objects and point cloud handling enabled: https://evaluation.tier4.jp/evaluation/reports/1a144b5e-943e-53d0-b7ab-7ea016e29f48?project_id=prd_jt

Effects on system behavior

It is confirmed that the changes do not affect the behavior when the point cloud handling is disabled.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

…er_pointcloud

satoshi-ota · 2024-05-01T01:20:15Z

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -87,6 +117,8 @@ struct Obstacle
  std::vector<PredictedPath> predicted_paths;
  double ego_to_obstacle_distance;
  double lat_dist_from_obstacle_to_traj;
+  PointCloud pointcloud;
+  bool pointcloud_repr;


Could you explain the purpose of this variable?

satoshi-ota · 2024-05-01T01:22:15Z

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

+    twist.linear.x = twist.linear.y = twist.angular.z = 0.0;
+
+    Eigen::Vector4d centroid;
+    pcl::compute3DCentroid(pointcloud, centroid);


Why did you calculate centroid of pointcloud?

…er_pointcloud

satoshi-ota · 2024-06-27T04:58:24Z

planning/autoware_obstacle_cruise_planner/src/node.cpp

+  if (
+    (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT &&
+     !isStopObstacle(obstacle.classification.label)) ||
+    (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_stop_)) {


[imo] it's a little bit difficult to understand this condition.

This is my suggestion.

if ( (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT && !isStopObstacle(obstacle.classification.label)) { return std::nullopt; } if ( (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_stop_)) { return std::nullopt; }

satoshi-ota · 2024-06-27T04:59:28Z

planning/autoware_obstacle_cruise_planner/src/node.cpp

+    !enable_slow_down_planning_ ||
+    (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT &&
+     !isSlowDownObstacle(obstacle.classification.label)) ||
+    (obstacle.source_type == Obstacle::SourceType::POINTCLOUD && !use_pointcloud_for_slow_down_)) {


same as https://github.com/autowarefoundation/autoware.universe/pull/6907/files#r1656411386.

codecov · 2024-07-03T22:13:18Z

Codecov Report

Attention: Patch coverage is 18.39080% with 142 lines in your changes missing coverage. Please review.

Project coverage is 28.32%. Comparing base (c9aa86c) to head (8e88283).
Report is 1 commits behind head on main.

Files	Patch %	Lines
...ning/autoware_obstacle_cruise_planner/src/node.cpp	19.13%	126 Missing and 5 partials ⚠️
...utoware/obstacle_cruise_planner/common_structs.hpp	0.00%	10 Missing ⚠️
..._obstacle_cruise_planner/src/planner_interface.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6907      +/-   ##
==========================================
- Coverage   28.69%   28.32%   -0.37%     
==========================================
  Files        1587     1585       -2     
  Lines      116143   115614     -529     
  Branches    49559    49305     -254     
==========================================
- Hits        33326    32750     -576     
- Misses      73723    73871     +148     
+ Partials     9094     8993     -101

Flag	Coverage Δ		*Carryforward flag
differential	`6.62% <18.39%> (?)`
total	`28.34% <ø> (-0.35%)`	⬇️	Carriedforward from 6a70824

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

mitukou1109 added 12 commits April 24, 2024 17:32

add pointcloud to obstacle properties

e264cf3

add tf listener & pointcloud subscriber

60224e1

add parameters for pointcloud obstacle

028e665

add type aliases

2f054cd

convert pointcloud to obstacle

c41ded8

Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…

84a31a9

…er_pointcloud

add type alias

a19f529

add polygon conversion for pointcloud obstacle

29f9284

initialize twist & pose of pointcloud obstacle

ce95e63

overload to handle both obstacle & predicted path

30ccefe

implement ego behavior determination against pointcloud obstacles

23c1fff

Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…

fcbb887

…er_pointcloud

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label May 1, 2024

satoshi-ota reviewed May 1, 2024

View reviewed changes

mitukou1109 added 7 commits May 1, 2024 11:59

generate obstacle from point

8924771

revert getCollisionIndex()

0d99190

generate obstacle from each point in cloud

035ad99

set pointcloud obstacle velocity to 0

b724add

use tf buffer & listener with pointers

1a3151c

update latest pointcloud data

7460587

add topic remap

8df16ff

github-actions bot added the component:launch Launch files, scripts and initialization tools. (auto-assigned) label May 1, 2024

mitukou1109 and others added 8 commits May 1, 2024 17:01

remove unnecessary includes

fede0ff

set slow down obstacle velocity to 0

25731b3

add flag to consider pointcloud obstacle for stopping & slowing down

c57545d

style(pre-commit): autofix

3b6a30c

Merge remote-tracking branch 'origin/main' into obstacle_cruise_plann…

db48811

…er_pointcloud

downsample pointcloud using voxel grid

844393a

change shape type of pointcloud obstacle to polygon

4c7a339

convert pointcloud to obstacle by clustering

fb00bed

mitukou1109 requested review from takayuki5168, rej55, kyoichi-sugahara, mkuri, soblin, shmpwk, taikitanaka3, TakaHoribe, TomohitoAndo, tkimura4 and zulfaqar-azmi-t4 as code owners June 27, 2024 02:32

mitukou1109 added 2 commits June 27, 2024 13:53

make nest shallower

61ee56a

Merge remote-tracking branch 'origin/awf-latest' into tmp/shallow_nest

5a44be7

satoshi-ota reviewed Jun 27, 2024

View reviewed changes

define vector concatenate function

9b313dd

mitukou1109 mentioned this pull request Jun 27, 2024

feat(obstacle_cruise_planner): support pointcloud-based obstacles autowarefoundation/autoware_launch#980

Merged

7 tasks

yuki-takagi-66 approved these changes Jun 27, 2024

View reviewed changes

satoshi-ota approved these changes Jul 2, 2024

View reviewed changes

satoshi-ota added tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) and removed tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) labels Jul 2, 2024

mitukou1109 added 2 commits July 2, 2024 14:46

shorten variable names

a4a5986

fix redundant condition

7f227b6

satoshi-ota added the tag:run-clang-tidy-differential Mark to enable clang-tidy-differential workflow. (used-by-ci) label Jul 2, 2024

satoshi-ota added 2 commits July 2, 2024 16:37

Merge branch 'main' into obstacle_cruise_planner_pointcloud

c3c9532

Merge branch 'main' into obstacle_cruise_planner_pointcloud

8e88283

satoshi-ota added tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) and removed tag:run-clang-tidy-differential Mark to enable clang-tidy-differential workflow. (used-by-ci) labels Jul 3, 2024

satoshi-ota merged commit 9e47b4d into autowarefoundation:main Jul 3, 2024
49 of 51 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(obstacle_cruise_planner): support pointcloud-based obstacles #6907

feat(obstacle_cruise_planner): support pointcloud-based obstacles #6907

mitukou1109 commented May 1, 2024 •

edited

Loading

satoshi-ota May 1, 2024

satoshi-ota May 1, 2024

satoshi-ota Jun 27, 2024

satoshi-ota Jun 27, 2024

codecov bot commented Jul 3, 2024 •

edited

Loading

feat(obstacle_cruise_planner): support pointcloud-based obstacles #6907

feat(obstacle_cruise_planner): support pointcloud-based obstacles #6907

Conversation

mitukou1109 commented May 1, 2024 • edited Loading

Description

Tests performed

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

satoshi-ota May 1, 2024

Choose a reason for hiding this comment

satoshi-ota May 1, 2024

Choose a reason for hiding this comment

satoshi-ota Jun 27, 2024

Choose a reason for hiding this comment

satoshi-ota Jun 27, 2024

Choose a reason for hiding this comment

codecov bot commented Jul 3, 2024 • edited Loading

Codecov Report

mitukou1109 commented May 1, 2024 •

edited

Loading

codecov bot commented Jul 3, 2024 •

edited

Loading